"""
2D Controller Class to be used for the CARLA waypoint follower demo.
"""
import numpy as np


class CUtils(object):
    def __init__(self):
        pass

    def create_var(self, var_name, value):
        if not var_name in self.__dict__:
            self.__dict__[var_name] = value


class Controller2D(object):
    def __init__(self, waypoints):
        self.vars = CUtils()
        self._current_x = 0
        self._current_y = 0
        self._current_yaw = 0
        self._current_speed = 0
        self._desired_speed = 0
        self._current_frame = 0
        self._current_timestamp = 0
        self._start_control_loop = False
        self._set_throttle = 0
        self._set_brake = 0
        self._set_steer = 0
        self._waypoints = waypoints
        self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
        self._pi = np.pi
        self._2pi = 2.0 * np.pi

    def update_values(self, x, y, yaw, speed):
        self._current_x = x
        self._current_y = y
        self._current_yaw = yaw
        self._current_speed = speed
        self._start_control_loop = True

    def update_desired_speed(self):
        min_idx = 0
        min_dist = float("inf")
        desired_speed = 0
        for i in range(len(self._waypoints)):
            dist = np.linalg.norm(np.array([
                self._waypoints[i][0] - self._current_x,
                self._waypoints[i][1] - self._current_y]))
            if dist < min_dist:
                min_dist = dist
                min_idx = i
        if min_idx < len(self._waypoints) - 1:
            desired_speed = self._waypoints[min_idx][2]
        else:
            desired_speed = self._waypoints[-1][2]
        self._desired_speed = desired_speed
        # print("desired_speed:", desired_speed)

    def update_waypoints(self, new_waypoints):
        self._waypoints = new_waypoints

    def get_commands(self):
        return self._set_throttle, self._set_steer, self._set_brake

    def set_throttle(self, input_throttle):
        # Clamp the throttle command to valid bounds
        throttle = np.fmax(np.fmin(input_throttle, 1.0), 0.0)
        self._set_throttle = throttle

    def set_steer(self, input_steer_in_rad):
        # Covnert radians to [-1, 1]
        input_steer = self._conv_rad_to_steer * input_steer_in_rad

        # Clamp the steering command to valid bounds
        steer = np.fmax(np.fmin(input_steer, 1.0), -1.0)
        self._set_steer = steer

    def set_brake(self, input_brake):
        # Clamp the steering command to valid bounds
        brake = np.fmax(np.fmin(input_brake, 1.0), 0.0)
        self._set_brake = brake

    def update_controls(self):
        ######################################################
        # RETRIEVE SIMULATOR FEEDBACK
        ######################################################
        x = self._current_x
        y = self._current_y
        yaw = self._current_yaw
        v = self._current_speed
        self.update_desired_speed()
        v_desired = self._desired_speed
        t = self._current_timestamp
        waypoints = self._waypoints
        steer_output = 0
        error_0 = 0

        self.vars.create_var('v_previous', 0.0)
        self.vars.create_var('error_1', 0.0)
        self.vars.create_var('error_2', 0.0)
        self.vars.create_var('steer_1', 0.0)

        # Skip the first frame to store previous values properly
        if self._start_control_loop:

            throttle_output = 0.8
            brake_output = 0

            # These are the constants generated by the root locus method
            Kp = 1.7
            Ki = 0.02
            Kd = 1.0

            desired_yaw = np.arctan2((y - waypoints[-1][1]), (x - waypoints[-1][0]))
            # print("desired_yaw:", desired_yaw)
            # print("yaw:", yaw)

            error_0 = desired_yaw - (yaw)
            a1 = -1
            b0 = Kp + Ki + Kd
            b1 = -(Kp + 2 * Kd)
            b2 = Kd

            steer_output = b0 * error_0 + b1 * self.vars.error_1 + b2 * self.vars.error_2 - a1 * self.vars.steer_1
            steer_output = -steer_output
            # Uncomment the following line to add random disturbance
            # steer_output = steer_output + np.random.uniform(-0.1, 0.1)
            # print(str(np.round(yaw, decimals=5)) + " " + str(np.round(steer_output, decimals=3)) + " " + str(
            #     np.round(desired_yaw, decimals=5)))

            ######################################################
            # SET CONTROLS OUTPUT
            ######################################################
            self.set_throttle(throttle_output)  # in percent (0 to 1)
            self.set_steer(steer_output)  # in rad (-1.22 to 1.22)
            self.set_brake(brake_output)  # in percent (0 to 1)

        self.vars.v_previous = v  # Store forward speed to be used in next step
        self.vars.error_2 = self.vars.error_1
        self.vars.error_1 = error_0
        self.vars.steer_1 = steer_output